﻿#include <xy/math/diff.h>
#include <xy/solver/linear.h>
#include <xy/solver/nonlinear.h>

namespace xy
{

namespace solver
{

using namespace xy::math;

double nonlinear::solve(func11 f, double x)
{
    static constexpr double h = numeric::distance;
    double dx = 0;
    for (std::size_t i = 0; i < numeric::iter; i++)
    {
        double fx = f(x);
        double j = (f(x + h) - f(x - h)) / (2 * h);

        // 迭代推进 x = x - f / f'
        dx = fx / j;
        x = x - dx;

        // 当变化量足够小或者是根就退出
        if (std::min(std::abs(dx), std::abs(fx)) < numeric::distance)
            break;
    }

    return x;
}

vecxd nonlinear::solve(funcxx f, vecxd x)
{
    for (std::size_t i = 0; i < numeric::iter; i++)
    {
        // 获得 Jacobi 矩阵
        matxd J = diff::jacobian(f, x);
        if (J.norm() < std::numeric_limits<double>::epsilon())
            break;

        // JT*Jdx = -JT*f
        vecxd fx = f(x).transpose() * J; // 变为行向量
        matxd JTJ = J.transposed() * J;

        linear_ptlu solver(JTJ);
        solver.perform();

        auto dx = solver.solve(matxd(fx));
        x -= dx.transposed();

        // 当变化量足够小或者是根就退出
        if (std::min(dx.norm(), fx.norm()) < numeric::distance)
            break;
    }

    return x;
}

} // namespace solver

} // namespace xy
